
// ===============================================================================================================
// -*- C++ -*-
//
// MathLib.cpp - Fast 3D math routines.
//
// Copyright (c) 2011 Guilherme R. Lampert
// guilherme.ronaldo.lampert@gmail.com
//
// This code is licenced under the MIT license.
//
// This software is provided "as is" without express or implied
// warranties. You may freely copy and compile this source into
// applications you distribute provided that the copyright text
// above is included in the resulting source code.
//
// ===============================================================================================================

#include <MathLib.hpp>

// C++ std includes
#include <cstdlib>
#include <cmath>
#include <ctime>

namespace MathLib {

// Definitions of useful mathematical constants:

const float PI = float(4.0 * atan(1.0));
const float TWO_PI  = (float(2.0) * PI);
const float HALF_PI = (float(0.5) * PI);
const float INV_PI  = (float(1.0) / PI);
const float INV_TWO_PI = (float(1.0) / TWO_PI);
const float DEG_TO_RAD = (PI / float(180.0));
const float RAD_TO_DEG = (float(180.0) / PI);

// Math definitions:

// Note: Here I use some machine code black magic in a couple of functions ;)
// Right now I do not provide any other alternative for other platforms/compilers, just the x86 asm code
// for visual c++. So if you can't/don't want to use it you will just have to replace it by yourself.

float Sine(float x)
{
#if defined (_MSC_VER)

	float result;

	__asm fld dword ptr [x]
	__asm fsin
	__asm fstp dword ptr [result]

	return (result);

#else
#error Inline assembly only available for X86 VS builds !
#endif // _MSC_VER
}

float Cosine(float x)
{
#if defined (_MSC_VER)

	float result;

	__asm fld dword ptr [x]
	__asm fcos
	__asm fstp dword ptr [result]

	return (result);

#else
#error Inline assembly only available for X86 VS builds !
#endif // _MSC_VER
}

void SineCosine(float ang, float & s, float & c)
{
#if defined (_MSC_VER)

	__asm fld dword ptr [ang]
	__asm fsincos // fsincos gives both the sine and cosine of an angle in one shot!

	__asm mov eax, dword ptr [s]
	__asm mov ebx, dword ptr [c]

	__asm fstp dword ptr [ebx]
	__asm fstp dword ptr [eax]

#else
#error Inline assembly only available for X86 VS builds !
#endif // _MSC_VER
}

float ArcSine(float x)
{
	// Clamp input to [-1,1]

	if (x <= float(-1.0))
	{
		return (float(-HALF_PI));
	}
	else if (x >= float(1.0))
	{
		return (float(HALF_PI));
	}
	else
	{
		return (float(asin(double(x))));
	}
}

float ArcCosine(float x)
{
	// Clamp value to [-1,1]

	if (x <= float(-1.0))
	{
		return (float(PI));
	}
	else if (x >= float(1.0))
	{
		return (float(0.0));
	}
	else
	{
		return (float(acos(double(x))));
	}
}

float Tangent(float x)
{
	return (float(tan(double(x))));
}

float ArcTangent(float x)
{
	return (float(atan(double(x))));
}

float ArcTangent(float x, float y)
{
	return (float(atan2(double(x), double(y))));
}

float Hypotenuse(float x, float y)
{
	return (Sqrt((x * x) + (y * y)));
}

float Sqrt(float x)
{
#if defined (_MSC_VER)

	float result;

	// Fast SSE square root:
	__asm sqrtss xmm0, dword ptr [x]
	__asm movss dword ptr [result], xmm0

	return (result);

#else
#error Inline assembly only available for X86 VS builds !
#endif // _MSC_VER
}

float InvSqrt(float x)
{
#if defined (_MSC_VER)

	float result;

	// Fast SSE reciprocal square root:
	__asm rsqrtss xmm0, dword ptr [x]
	__asm movss dword ptr [result], xmm0

	return (result);

#else
#error Inline assembly only available for X86 VS builds !
#endif // _MSC_VER
}

float Ceil(float x)
{
	return (float(ceil(double(x))));
}

float Floor(float x)
{
	return (float(floor(double(x))));
}

int Truncate(float x)
{
#if defined (_MSC_VER)

	int result;

	__asm fld dword ptr [x]
	__asm fistp dword ptr [result]

	return (result);

#else
#error Inline assembly only available for X86 VS builds !
#endif // _MSC_VER
}

bool IsNAN(float x)
{
	// Cool FP trick by John Carmack!
	// NANs never compare equal.
	return (x != x);
}

void SeedRandomGenerator(void)
{
	time_t now = time(0);
	unsigned char * p = reinterpret_cast<unsigned char *>(&now);
	unsigned int seed = 0;

	for (size_t i = 0; i < sizeof(now); ++i)
	{
		seed = seed * (0xff + 2U) + p[i];
	}

	srand(seed);
}

float UniformRandom(void)
{
	static const float oneOverRndMax = (1.0f / (RAND_MAX + 1.0f));
	return (static_cast<float>(rand() * oneOverRndMax));
}

int RandomNumber(int min, int max)
{
	return (static_cast<int>(min + UniformRandom() * (max - min)));
}

Vec2f LinearInterpolation(const Vec2f & a, const Vec2f & b, float t)
{
	return (Vec2f(a.x + t * (b.x - a.x),
				  a.y + t * (b.y - a.y)));
}

Vec3f LinearInterpolation(const Vec3f & a, const Vec3f & b, float t)
{
	return (Vec3f(a.x + t * (b.x - a.x),
				  a.y + t * (b.y - a.y),
				  a.z + t * (b.z - a.z)));
}

Vec3f HermiteInterpolation(const Vec3f & y0, const Vec3f & y1, const Vec3f & y2, const Vec3f & y3, float t, float tension, float bias)
{
	Vec3f v;
	float m0, m1, a0, a1, a2, a3;

	const float oneMinusTension = (1.0f - tension);
	const float mu2 = (t * t);
	const float mu3 = (mu2 * t);

	// X
	m0  = (y1.x - y0.x) * (1.0f + bias) * oneMinusTension * 0.5f;
	m0 += (y2.x - y1.x) * (1.0f - bias) * oneMinusTension * 0.5f;
	m1  = (y2.x - y1.x) * (1.0f + bias) * oneMinusTension * 0.5f;
	m1 += (y3.x - y2.x) * (1.0f - bias) * oneMinusTension * 0.5f;
	a0 = 2.0f * mu3 - 3.0f * mu2 + 1.0f;
	a1 = mu3 - 2.0f * mu2 + t;
	a2 = mu3 - mu2;
	a3 = -2.0f * mu3 + 3.0f * mu2;
	v.x = (a0 * y1.x + a1 * m0 + a2 * m1 + a3 * y2.x);

	// Y
	m0  = (y1.y - y0.y) * (1.0f + bias) * oneMinusTension * 0.5f;
	m0 += (y2.y - y1.y) * (1.0f - bias) * oneMinusTension * 0.5f;
	m1  = (y2.y - y1.y) * (1.0f + bias) * oneMinusTension * 0.5f;
	m1 += (y3.y - y2.y) * (1.0f - bias) * oneMinusTension * 0.5f;
	a0 = 2.0f * mu3 - 3.0f * mu2 + 1.0f;
	a1 = mu3 - 2.0f * mu2 + t;
	a2 = mu3 - mu2;
	a3 = -2.0f * mu3 + 3.0f * mu2;
	v.y = (a0 * y1.y + a1 * m0 + a2 * m1 + a3 * y2.y);

	// Z
	m0  = (y1.z - y0.z) * (1.0f + bias) * oneMinusTension * 0.5f;
	m0 += (y2.z - y1.z) * (1.0f - bias) * oneMinusTension * 0.5f;
	m1  = (y2.z - y1.z) * (1.0f + bias) * oneMinusTension * 0.5f;
	m1 += (y3.z - y2.z) * (1.0f - bias) * oneMinusTension * 0.5f;
	a0 = 2.0f * mu3 - 3.0f * mu2 + 1.0f;
	a1 = mu3 - 2.0f * mu2 + t;
	a2 = mu3 - mu2;
	a3 = -2.0f * mu3 + 3.0f * mu2;
	v.z = (a0 * y1.z + a1 * m0 + a2 * m1 + a3 * y2.z);

	return (v);
}

}; // namespace MathLib {}